#include <iostream>
#include "stdio.h"
#include "horizon.h"
#include "../utils/stablization_param.h"
#include "../utils/keyframe.h"

#if 0

Vector3 z_axis = {0.0, 0.0, 1.0};
Vector3 y_axis = {0.0, 1.0, 0.0};

HorizonLock horizonlock_default() {
    HorizonLock lock;
    lock.lock_enabled = false;
    lock.horizonlockpercent = 100.0;
    lock.horizonroll = 0.0;
    return lock;
}

//输入必须是归一化的四元数
Quaternion lock_horizon_angle(const Quaternion *q, double roll_correction) {
    // Define unit vectors
    Vector3 x_axis = {1.0, 0.0, 0.0};
    Vector3 y_axis = {0.0, 1.0, 0.0};
    Vector3 z_axis = {0.0, 0.0, 1.0};

    // Calculate pitch and yaw
    Vector3 test_vec;
    test_vec.x = q->q0 * z_axis.x + q->q1 * z_axis.y + q->q2 * z_axis.z;
    test_vec.y = q->q0 * z_axis.y - q->q1 * z_axis.x - q->q3 * z_axis.z;
    test_vec.z = q->q0 * z_axis.z + q->q1 * z_axis.y - q->q2 * z_axis.x;

    double pitch = -asin(test_vec.z);
    double yaw = atan2(test_vec.y, test_vec.x);

    // Calculate rotation quaternions
    Quaternion rot_yaw = {cos(yaw / 2), 0.0, sin(yaw / 2), 0.0};
    Quaternion rot_pitch = {cos(pitch / 2), sin(pitch / 2), 0.0, 0.0};
    Quaternion rot_roll = {cos(roll_correction / 2), 0.0, 0.0, sin(roll_correction / 2)};

    // Define initial_quat
    Quaternion initial_quat = {cos(M_PI_2 / 2), 0.0, 0.0, sin(M_PI_2 / 2)}; // Quaternion from_axis_angle(&y_axis, std::f64::consts::FRAC_PI_2) in Rust

    // Calculate final quaternion
    Quaternion result;
    result.q0 = initial_quat.q0 * rot_yaw.q0 - initial_quat.q1 * rot_yaw.q1 - initial_quat.q2 * rot_yaw.q2 - initial_quat.q3 * rot_yaw.q3;
    result.q1 = initial_quat.q0 * rot_yaw.q1 + initial_quat.q1 * rot_yaw.q0 + initial_quat.q2 * rot_yaw.q3 - initial_quat.q3 * rot_yaw.q2;
    result.q2 = initial_quat.q0 * rot_yaw.q2 - initial_quat.q1 * rot_yaw.q3 + initial_quat.q2 * rot_yaw.q0 + initial_quat.q3 * rot_yaw.q1;
    result.q3 = initial_quat.q0 * rot_yaw.q3 + initial_quat.q1 * rot_yaw.q2 - initial_quat.q2 * rot_yaw.q1 + initial_quat.q3 * rot_yaw.q0;

    return result;
}

void set_horizon(HorizonLock *lock, double lock_percent, double roll) {
    lock->horizonroll = roll;
    lock->horizonlockpercent = lock_percent;
    lock->lock_enabled = (lock->horizonlockpercent > 1e-6) ? 1 : 0;
}

uint64_t get_checksum(const HorizonLock *lock) {
    uint64_t checksum = 0;

    uint64_t bits_percent = *(uint64_t*)&lock->horizonlockpercent;
    uint64_t bits_roll = *(uint64_t*)&lock->horizonroll;

    checksum ^= bits_percent;
    checksum ^= bits_roll;

    return checksum;
}

Vector3 interpolate_gravity_vector(TimeVec* gvec, int64_t timestamp) {
    // Implement the interpolation logic here
    Vector3 result = {0.0, 1.0, 0.0}; // Placeholder, replace with actual logic
    return result;
}
//pub fn lock(  &self,
//              quats: &TimeQuat,
//              org_quats: &TimeQuat,
//              grav: &Option<crate::gyro_source::TimeVec>,
//              use_grav: bool,
//              _int_method: usize,
//              keyframes: &KeyframeManager,
//              params: &StabilizationParams)

TimeQuat lock(const TimeQuat* quats,
              const TimeQuat* org_quats,
              const TimeVec* grav,
              int use_grav,
              size_t int_method,
              const KeyframeManager* keyframes,
              const StabilizationParams* params,
              const HorizonLock* lock) {
        TimeQuat result;
        // Initialize and allocate memory for the result

        //  操作界面默认lock_enable 是false
        //  鉴于本方法嵌套太多
        //  因此，先跳过本方法的具体实现
}





int main() {
    // Create a default HorizonLock
    HorizonLock lock = horizonlock_default();

    // Use the lock and its members as needed
    printf("lock_enabled: %d\n", lock.lock_enabled);
    printf("horizonlockpercent: %lf\n", lock.horizonlockpercent);
    printf("horizonroll: %lf\n", lock.horizonroll);

    return 0;
}

#endif